import sensor, image, time,pyb,ustruct,math
from image import SEARCH_EX, SEARCH_DS,
from pyb import UART,LED,Pin
uart = UART(3, 115200)
uart.init(115200, bits=8, parity=None, stop=1)
green_threshold   = (30, 100, 15, 127, 15, 127)
red_led = pyb.LED(1)
green_led = pyb.LED(2)
bule_led = pyb.LED(3)
green_led.on()
sensor.reset()
sensor.set_contrast(1)
sensor.set_gainceiling(16)
sensor.set_pixformat(sensor.GRAYSCALE)
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(10)
sensor.set_auto_whitebal(False)
clock = time.clock()
roi1 =   [(5,87,24,28),
		  (130,92,23,24),
		  (38,56,72,59)]
templates = ["/1.pgm", "/2.pgm", "/3.pgm", "/4.pgm","/5.pgm","/6.pgm","/7.pgm"]
def find_max(blobs):
	max_size=0
	for blob in blobs:
		if blob.pixels() > max_size:
			max_blob=blob
			max_size = blob.pixels()
	return max_blob
flag1 = 0
flag11 = 0
flag_digital = 0
time.sleep_ms(2000)
while(True):
	clock.tick()
	img = sensor.snapshot()
	for t in templates:
		template = image.Image(t)
		r = img.find_template(template, 0.60,step=4, search=SEARCH_EX)
		if r:
			if t==templates[0]:
				green_led.off()
				bule_led.on()
				flag1=1
				flag11=1
				flag_digital = 1
				img.draw_rectangle(r)
				uart.write('aaaaa')
				print("a")
				break
		if r:
			if t==templates[1]:
				bule_led.on()
				green_led.off()
				flag1=1
				flag11=1
				flag_digital = 2
				img.draw_rectangle(r)
				uart.write('bbbbbb')
				print("b")
				break
		if r:
			if t==templates[2]:
				bule_led.on()
				green_led.off()
				flag1=1
				flag_digital = 3
				img.draw_rectangle(r)
				uart.write('ccccc')
				print("c")
				break
		if r:
			if t==templates[3]:
				bule_led.on()
				green_led.off()
				flag1=1
				flag_digital = 4
				img.draw_rectangle(r)
				uart.write('dddddd')
				print("d")
				break
	if flag1 == 1:
		break
sensor.set_pixformat(sensor.RGB565)
flag = 1
time.sleep_ms(2000)
while(True):
	if flag and flag11 != 1:
		sensor.set_pixformat(sensor.GRAYSCALE)
		clock.tick()
		img = sensor.snapshot()
		for t in templates:
			template = image.Image(t)
			r = img.find_template(template, 0.60,roi=(1, 2, 158, 85),step=4, search=SEARCH_EX)
			if r:
				if (t==templates[2] or t==templates[4] or t==templates[5] or t==templates[6]) and flag_digital == 3:
					if (r[0]+r[2])/2<71:
						a = (r[0]+r[2])/2
						uart.write('l')
					if (r[0]+r[2])/2>71:
						a = (r[0]+r[2])/2
						uart.write('r')
					bule_led.off()
					red_led.on()
					flag = 0
					print(r)
				elif t==templates[3] and flag_digital == 4:
					if (r[0]+r[2])/2<71:
						a = (r[0]+r[2])/2
						uart.write('l')
					if (r[0]+r[2])/2>71:
						a = (r[0]+r[2])/2
						uart.write('r')
					bule_led.off()
					red_led.on()
					flag = 0
	sensor.set_pixformat(sensor.RGB565)
	img = sensor.snapshot()
	blobs1 = img.find_blobs([green_threshold],invert = False,roi = (5,87,24,28))
	blobs2 = img.find_blobs([green_threshold],invert = False,roi = (112,92,23,24))
	blobs3 = img.find_blobs([green_threshold],invert = False,roi = (53,55,40,60))
	img.draw_rectangle(roi1[0], color=(255,0,0))
	img.draw_rectangle(roi1[1], color=(255,0,0))
	img.draw_rectangle(roi1[2], color=(255,0,0))
	if blobs1:
		b1 = find_max(blobs1)
		img.draw_rectangle(b1[0:4])
		if b1[4]>80:
			img.draw_rectangle(b1[0:4])
			print("左边",b1[4])
	if blobs2:
		b = find_max(blobs2)
		if b[4]>100:
			img.draw_rectangle(b[0:4])
			img.draw_cross(b[5], b[6])
			uart.write('#')
	if blobs3:
		b3 = find_max(blobs3)
		img.draw_rectangle(b3[0:4])
		output_str = "%d" % b3[5]
		uart.write('%'+output_str+'!')
	time.sleep_ms(10)